Handling Constrained Optimization in Factor Graphs for Autonomous Navigation

Factor graphs are graphical models used to represent a wide variety of problems across robotics, such as Structure from Motion (SfM), Simultaneous Localization and Mapping (SLAM) calibration. Typically, at their core, they have an optimization problem whose terms only depend on small subset variables. graph solvers exploit the locality drastically reduce computational time Iterative Least-Squares (ILS) methodology. Although extremely powerful, application is usually limited unconstrained problems. In this paper, we model constraints over variables within factor by introducing version method Lagrange Multipliers. We show potential our presenting full navigation stack based graphs. Differently standard stacks, can both optimal control for local planning localization with graphs, solve two using ILS validate approach in real-world autonomous scenarios, comparing it de facto implemented ROS. Comparative experiments that hand system outperforms nonlinear programming solver Interior-Point Optimizer (IPOPT) runtime, while achieving similar solutions.